from tools.lidar_cmd import *
from tools.serial_tools import serial_port
import time
import re
import sys

class rplidar_driver:
    '''
        封装的rplidar驱动
    '''
    def __init__(self,status='Free',baudrate=115200):
        self.sdk_version = '0.1.0'
        self.setStatus(status)
        self.health_status = -1
        self.port = serial_port(vid_pid='10C4:EA60',name='rplidar',baudrate=baudrate,write_mode='Specific')

    def setStatus(self,status):
        '''
            设置驱动的状态
        '''
        if status in ['Free','Work']:
            self.status = status
        else:
            raise RuntimeError('No such scan mode named {}.'.format(status))

    def getStatus(self):
        '''
            获取驱动的状态
        '''
        return self.status

    def healthOK(self):
        '''
            获取驱动的健康信息
        '''
        return True if self.health_status == 0 else False

    def sendCommand(self,cmd_name,cmd_proc_func):
        '''
            发送指令
            - params
                - cmd_name:指令的名称,见lidar_cmd.py
                - cmd_proc_func:对应指令需要执行的函数
        '''
        try:
            self.port.serialWrite(message=cmd[cmd_name])
        except KeyError:
            raise RuntimeError('No such command named {}.'.format(cmd_name))
        finally:
            if cmd_name in has_ans:
                time.sleep(0.1)
                data = self.port.serialRead()
                match= re.search(ans[cmd_name],data)
                if match:
                    cmd_proc_func(match,data)
                else:
                    raise IOError('Something wrong during serial port communication.')
            else:
                cmd_proc_func()

    def getInfo(self,match,data):
        '''
            获取设备信息指令的处理函数
            - params
                - match:返回的正则匹配信息
                - data:返回的报文
        '''
        index = match.span()[1]
        valid_data = data[index:-1]
        major_model = valid_data[0]
        sub_model = valid_data[1]
        firmware_minor = int(valid_data[2:4],16)
        firmware_major = int(valid_data[4:6],16)
        hardware_version = int(valid_data[6:8],16)
        serial_number = valid_data[8:].upper()
        print('RPLIDAR running on pyrplidar for {}. pyrplidar SDK Version : {}.'.format(sys.platform,self.sdk_version))
        print('RPLIDAR Serial Number: {}.'.format(serial_number))
        print('RPLIDAR type: A{}M{}.'.format(major_model,sub_model))
        print('Firmware Version: {}.{}.'.format(firmware_major,firmware_minor))
        print('Hardware Version: {}.'.format(hardware_version))

    def getHealth(self,match,data):
        '''
            获取健康信息指令的处理函数
            - params
                - match:返回的正则匹配信息
                - data:返回的报文
        '''
        index = match.span()[1]
        valid_data = data[index:-1]
        self.health_status = int(valid_data[0:2],16)
        error_code = int(valid_data[2:][::-1],16)
        print("Heatlh Status: {}.".format(self.health_status))
        if self.health_status != 0:
            print('Error Code: {}.'.format(error_code))
            if self.health_status == 1:
                raise Warning('WARNING: Potential error but not enough to cause RPLIDAR enter protective stop!')
            elif self.health_status == 2:
                raise RuntimeError('ERROR: RPLIDAR has entered protective stop!')
            else:
                raise RuntimeError('ERROR: Unknown Health Status!')
           
    def getSampleRate(self,match,data):
        '''
            获取采样率指令的处理函数
            - params
                - match:返回的正则匹配信息
                - data:返回的报文
        '''
        index = match.span()[1]
        valid_data = data[index:-1]
        texpress = int(valid_data[6:8]+valid_data[4:6],16)
        return texpress
    
    def stop(self):
        '''
            停止采样的处理函数
        '''
        self.setStatus('Free')

    def reset(self):
        '''
            测距核心软重启的处理函数
        '''
        time.sleep(0.05)
        self.setStatus('Free')

    def Scan(self,match,data):
        '''
            开始扫描采样的处理函数
        '''
        if self.health_status == 0:
            self.setStatus('Work')
            print('Start scan in express mode.')
        else:
            raise Warning('Health Status Error!')
    
    def getResponse(self,buflen=-1):
        '''
            获取串口应答报文
            - params
                - buflen 为-1则读取缓冲区长度信息，否则读取指定长度信息
        '''
        msg = self.port.serialRead(buflen=buflen)
        if len(msg) !=0:
            return msg[2:-1]
        else:
            return 'ffffffff'


if __name__ == '__main__':
    drv = rplidar_driver()
    drv.sendCommand('GETINFO',drv.getInfo)
    drv.sendCommand('GETHELH',drv.getHealth)
    drv.sendCommand('EXPSCAN',drv.Scan)
    status_ok = True if drv.getStatus() == 'Work' and drv.healthOK() else False
    while status_ok:
       data = drv.getResponse(buflen=84)
       print(data)